#ifndef __PIDCONTROLER___
#define __PIDCONTROLER__

#include <stdio.h>

using namespace gazebo;
class PIDControler
{
  private:
      //PID struct
      double P;
      double I;
      double D;
      double Expect;
      double Error;
      double Integration;
      double Error_last;

  public:
      //PID初始化函数
      void PID_init(double p,double i,double d,double expect);
      //PID公式
      double PID_calc(double feedback);
};

#endif
